#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <thread>
#include <vector>
#include <list>
#include <iostream>
#include "sensor_msgs/Imu.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/UInt32MultiArray.h"
#include "event_ros/CameraData.h"
#include "event_ros/Event.h"
#include "event_ros/EventVector.h"
#include "common_constants.h"

using namespace std;

/*
获取当前时间
@return:
    double 当前的时间戳（单位为秒）
*/
double time() {
    ros::Time rosTime = ros::Time::now();
    double res = rosTime.toSec();
    return res;
}

bool in(double cur, double min, double max) {
    return cur >= min && cur < max;
}

// 定义暂存数据的单元
list<event_ros::Event> eventBuffer;
bool eventLock = false;
list<sensor_msgs::Imu> imuBuffer;
bool imuLock = false;

// 定义传输数据的节点
ros::Publisher nodePub;
bool publisherInitialized = false;
ros::Subscriber eventSub;
ros::Subscriber imuSub;
bool subscriberInitialized = false;

double innerTSOrigin = -1.0; // 内部时间戳原点（内部时间戳为0时外部时间戳位置）
double lastInnerTS = 0.0; // 最后一个事件的内部时间戳
double maxInnerTS = 0.0; // 最大记录过的内部时间戳

double lastPublishTime = -1.0;

void publish() {
    double msgTime = time();
    event_ros::CameraData pubRes;
    std_msgs::UInt32MultiArray eventData;
    std_msgs::Float64MultiArray imuData;

    if (!eventBuffer.empty()) {
        // 将事件从buffer中复制出来，并清空buffer
        eventLock = true;
        vector<event_ros::Event> curEvents;
        for (list<event_ros::Event>::iterator it = eventBuffer.begin(); it != eventBuffer.end(); it++) {
            curEvents.push_back(*it);
        }
        eventBuffer.clear();
        // eventBuffer.shrink_to_fit();
        eventLock = false;

        for (vector<event_ros::Event>::iterator it = curEvents.begin(); it != curEvents.end(); it++) {
            event_ros::Event data = *it;

            // 校正内外时间戳
            // 相机有一个内时间戳（在0-INNER_MAX间循环），将其对齐至UNIX时间戳
            double eventInnerTS = data.off_pixel_timestamp * 1e-6;

            // 更新内部时间戳原点
            if (innerTSOrigin < 0.0) innerTSOrigin = msgTime - eventInnerTS;
            maxInnerTS = max(eventInnerTS, maxInnerTS);
            if (lastInnerTS - eventInnerTS >= 5.0 && lastInnerTS > maxInnerTS - 0.1 && eventInnerTS < 0.1) {
                innerTSOrigin += maxInnerTS;
            }
            lastInnerTS = eventInnerTS;
            
            // 对齐至外部时间戳
            double eventOuterTS = innerTSOrigin + eventInnerTS;

            // 将事件加入待发布的消息中
            uint32_t curSec = (uint32_t)eventOuterTS;
            uint32_t curNSec = (uint32_t)((eventOuterTS - curSec) * NSEC);
            uint32_t curP = data.polarity > 0 ? 1 : 0;
            uint32_t curY = HEIGHT - 1 - data.x;
            uint32_t curX = data.y;
            eventData.data.push_back(curSec);
            eventData.data.push_back(curNSec);
            eventData.data.push_back(curP);
            eventData.data.push_back(curY);
            eventData.data.push_back(curX);
        }
    }

    if (!imuBuffer.empty()) {
        // 将IMU从buffer中复制出来，并清空buffer
        imuLock = true;
        vector<sensor_msgs::Imu> curIMUs;
        for (list<sensor_msgs::Imu>::iterator it = imuBuffer.begin(); it != imuBuffer.end(); it++) {
            curIMUs.push_back(*it);
        }
        imuBuffer.clear();
        // imuBuffer.shrink_to_fit();
        imuLock = false;

        for (vector<sensor_msgs::Imu>::iterator it = curIMUs.begin(); it != curIMUs.end(); it++) {
            // 将IMU信息加入待发布的消息中
            sensor_msgs::Imu data = *it;
            double_t imuTimeStamp = data.header.stamp.sec + (data.header.stamp.nsec / NSEC);
            double_t imuOmegaZ = data.angular_velocity.z;
            double_t imuOmegaY = data.angular_velocity.y;
            double_t imuOmegaX = data.angular_velocity.x;
            double_t accelZ = data.linear_acceleration.z;
            double_t accelY = data.linear_acceleration.y;
            double_t accelX = data.linear_acceleration.x;
            imuData.data.push_back(imuTimeStamp);
            imuData.data.push_back(imuOmegaZ);
            imuData.data.push_back(imuOmegaY);
            imuData.data.push_back(imuOmegaX);
            imuData.data.push_back(accelZ);
            imuData.data.push_back(accelY);
            imuData.data.push_back(accelX);
        }
    }

    int finalEventSize = eventData.data.size();
    int finalIMUSize = imuData.data.size();

    pubRes.header.stamp.sec = (uint32_t)msgTime;
    pubRes.header.stamp.nsec = (uint32_t)((msgTime - pubRes.header.stamp.sec) * NSEC);
    pubRes.header.frame_id = "{\"path\":null,\"env\":true}";
    if (finalEventSize) {
        pubRes.events.data = eventData.data;
    }
    pubRes.events.column.push_back("t_sec");
    pubRes.events.column.push_back("t_nsec");
    pubRes.events.column.push_back("p");
    pubRes.events.column.push_back("y");
    pubRes.events.column.push_back("x");
    if (finalIMUSize) {
        pubRes.imu.data = imuData.data;
    }
    pubRes.imu.column.push_back("t");
    pubRes.imu.column.push_back("angvel_z");
    pubRes.imu.column.push_back("angvel_y");
    pubRes.imu.column.push_back("angvel_x");
    pubRes.imu.column.push_back("accel_z");
    pubRes.imu.column.push_back("accel_y");
    pubRes.imu.column.push_back("accel_x");
    if (publisherInitialized){
        nodePub.publish(pubRes);
    }
}

/*
事件的回调函数
@params:
    msg: event_ros::EventVector 收到的（多个事件）消息
*/
void eventCallback(const event_ros::EventVector& msg) {
    vector<event_ros::Event> events = msg.events;
    // 采样比率，如果事件数太多，将其过滤至FINAL_EVENTS个
    int sampleRatio = max((int)(events.size() / FINAL_EVENTS), 1);
    int idx = 0;
    for (vector<event_ros::Event>::iterator it = events.begin(); it != events.end(); it++) {
        if ((idx % sampleRatio == 0) && !eventLock) {
            eventBuffer.push_back(*it);
        }
        idx++;
    }
    // cout << idx << ", " << sampleRatio << endl;
}

/*
IMU的回调函数
@params:
    msg: sensor_msgs::Imu 收到的IMU消息（角速度等）
*/
void imuCallback(const sensor_msgs::Imu& msg) {
    if (!imuLock){
        imuBuffer.push_back(msg);
    }
}

/*
订阅者的线程函数，用于执行订阅者线程
*/
void subThreadFunc() {
    ros::NodeHandle n;
    eventSub = n.subscribe("/celex5_mipi/events", 1, eventCallback);  // 订阅事件消息
    imuSub = n.subscribe("/celex5_mipi/imu", 1, imuCallback);  // 订阅IMU消息
    subscriberInitialized = true;
    ros::spin();  // 开始循环监听消息
}

/*
发布者的线程函数，用于发布本结点的执行结果
*/
void pubThreadFunc() {
    ros::NodeHandle n;
    nodePub = n.advertise<event_ros::CameraData>("/celex5_mipi/camera_data", 1);
    publisherInitialized = true;
    
    ros::Rate frequency(USEC / DURATION_USEC);
    while (ros::ok()) {
        publish();
        frequency.sleep();
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "cam_celex5_sub");  // 初始化ROS环境

    thread subThread(subThreadFunc);  // 定义订阅者线程
    thread pubThread(pubThreadFunc);  // 定义发布者线程

    subThread.join();  // 加入订阅者线程
    pubThread.join();  // 加入发布者线程

    return 0;
}